
double PID_Ctrl (double e1
                ,double kp = 0.00001
                ,double ki = 0.0001
                ,double kd = 0.005) {

   static 
   double e0 = e1;

   static
   double integral = 0;

   if (std::isnan (e1)) { e0 = integral = 0; return 0;}

   double ctrl =   (e1       * kp) 
                 + (integral * ki) 
                 + ((e1-e0)  * kd);

   integral = integral + 0.5 * (e0 + e1);

   e0 = e1;

   return ctrl;
            
}

